/*
 * Copyright (c) 2011, Fredrik Häggström
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Luleå University of Technology nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <tT.h>
#include <env.h>


#include "mcpwm.h"
#include "hsadc.h"
#include "adc.h"
#include "can.h"
#include "transive.h"
#include "measure.h"
#include "pid.h"
#include "dtc.h"

/******************************************************/



adc_t adc = adc();
hsadc_t hsadc = hsadc();
can_t can = can(500000);
transive_t transive = transive();
mcpwm_t mcpwm = mcpwm();
dtc_t dtc = dtc();


/******************************************************/
#define pid_target	&mcpwm
#define pid_func		mcpwm_set_duty
#define pid_min			-600
#define pid_max			600
#define pid_K				1
#define pid_Ti			0.003
#define pid_Td			0
#define pid_dt			0.001			// 100 micro sec
#define pid_input		&(hsadc.value[1])
pid_t pid = pid(pid_target, pid_func, pid_min, pid_max, pid_K, pid_Ti, pid_Td, pid_dt, pid_input);


/******************************************************
 * Initialize a CAN listner for the PID regulator
 *****************************************************/
can_listner_t pid_cur_param_listner = listner( 
																		&pid, 								// Object
																		pid_decode,		 				// Method
																		0x00000112,						// ID
																		CAN_EXT_ID_FORMAT,		// Format
																		0x00000100,						// Index
																		0xFFFFFF00						// Mask
																		);
																		
																		
/******************************************************
 * Initialize a CAN listner for the MCPWM
 *****************************************************/
can_listner_t mcpwm_param_listner = listner( 
																		&mcpwm, 							// Object
																		mcpwm_decode, 				// Method
																		0x00000112,						// ID
																		CAN_EXT_ID_FORMAT,		// Format
																		0x00000200,						// Index
																		0xFFFFFF00						// Mask
																		);
																		
/******************************************************
 * Initialize a CAN SYNC
 *****************************************************/
can_listner_t can_sync_listner = listner( 
																		&can, 								// Object
																		can_sync, 						// Method
																		0x00000100,						// ID
																		CAN_EXT_ID_FORMAT,		// Format
																		0x00000000,						// Index
																		0xFFFFFFFF						// Mask
																		);



uint32_t mode = 0;

static void init(void)
{


  ENV_DEBUG("enter init()\r\n");
	
	TT_OBJECT_CEIL(&mcpwm, 10);
	TT_OBJECT_CEIL(&pid, 10);
	TT_OBJECT_CEIL(&hsadc, 10);
	TT_OBJECT_CEIL(&adc, 10);
	TT_OBJECT_CEIL(&can, 10);
  TT_OBJECT_CEIL(&transive, 10);
	
	
	/* Initialize objects */
 	can_init(&can, &(can.baudrate));
	can_installer(&can, &can_sync_listner);
  can_installer(&can, &pid_cur_param_listner); 
  can_installer(&can, &mcpwm_param_listner);
	

	// Initialize the High Speed ADC  
	hsadc_init(&hsadc, TT_ARGS_NONE);
	adc_init(&adc, TT_ARGS_NONE);
	
	// Calibrate the current measurment for GND
	adc_calibrate(&adc, 6);
	adc_calibrate(&adc, 7);
	
	
	// Calibrate the current measurment for the U-leg
	// This pulls the U-leg low
	if (hsadc_calibrate0(&hsadc, TT_ARGS_NONE)) 
	{
		ENV_DEBUG("HSADC0 is not working!\r\n");
		hsadc.disabled |= (1<<0);
	}
	adc_calibrate(&adc, 2);
	adc_calibrate(&adc, 3);
	
	// Calibrate the current measurment for the V-leg
	// This pulls the V-leg low
	if (hsadc_calibrate1(&hsadc, TT_ARGS_NONE)) 
	{
		ENV_DEBUG("HSADC1 is not working!\r\n");
		hsadc.disabled |= (1<<1);
	}
	adc_calibrate(&adc, 4);
	adc_calibrate(&adc, 5);
	
	// Initialize the Motor Controller PWM
	// This releases all legs
  mcpwm_init( &mcpwm, TT_ARGS_NONE);	
	

  
  
  
  
  //test.id = 0xFFFFFFFF;
  //test.data[0] = (0x01<<0) | (0x23<<8) | (0x45<<16) | (0x67<<24);
  //test.data[1] = (0x89<<0) | (0xAB<<8) | (0xCD<<16) | (0xEF<<24);		
  //test.len = 8; 
  //test.format = 0; 
  //test.type = 0;							

  //TT_AFTER_BEFORE(100000000,1000, &can, can_send,&test);
	
  //mcpwm_enable(&mcpwm, TT_ARGS_NONE);
  
  //dtc_init(&dtc,TT_ARGS_NONE);
  
  //transive_init( &transive, TT_ARGS_NONE);
  //int32_t temp = 0;
  //mcpwm_set_duty(&mcpwm, &temp);
  

  TT_WITHIN(100000, 10000, &pid, pid_run, TT_ARGS_NONE);
 	
	TT_ASYNC(&hsadc, hsadc_start, TT_ARGS_NONE);
}

ENV_STARTUP(init);
